#include "stdafx.h"

#include "NIConverter.h"
#include <math.h>
#include <stdint.h>

#define CVT_DEPTH_FOV_HORIZONTAL 1.02101761
#define CVT_DEPTH_FOV_VERTICAL	 0.795870139
#define CVT_COLOR_FOV_HORIZONTAL 1.08210414
#define CVT_COLOR_FOV_VERTICAL	 0.848230016

NIConverter::NIConverter(void)
{
	refreshConvertCache();
}


NIConverter::~NIConverter(void)
{
}

void NIConverter::refreshConvertCache()
{
	m_world_DepthCache.xzFactor = float(tan(CVT_DEPTH_FOV_HORIZONTAL / 2) * 2);
	m_world_DepthCache.yzFactor = float(tan(CVT_DEPTH_FOV_VERTICAL / 2) * 2);
	m_world_DepthCache.resolutionX = 640;
	m_world_DepthCache.resolutionY = 480;
	m_world_DepthCache.halfResX = 320;
	m_world_DepthCache.halfResY = 240;
	m_world_DepthCache.coeffX = m_world_DepthCache.resolutionX / m_world_DepthCache.xzFactor;
	m_world_DepthCache.coeffY = m_world_DepthCache.resolutionY / m_world_DepthCache.yzFactor;

	m_world_ColorCache.xzFactor = float(tan(CVT_COLOR_FOV_HORIZONTAL / 2) * 2);
	m_world_ColorCache.yzFactor = float(tan(CVT_COLOR_FOV_VERTICAL / 2) * 2);
	m_world_ColorCache.resolutionX = 640;
	m_world_ColorCache.resolutionY = 480;
	m_world_ColorCache.halfResX = 320;
	m_world_ColorCache.halfResY = 240;
	m_world_ColorCache.coeffX = m_world_ColorCache.resolutionX / m_world_ColorCache.xzFactor;
	m_world_ColorCache.coeffY = m_world_ColorCache.resolutionY / m_world_ColorCache.yzFactor;
}

void NIConverter::convertDepthToWorldCoordinates(float depthX, float depthY, float depthZ, float* pWorldX, float* pWorldY, float* pWorldZ)
{
	float normalizedX = depthX / m_world_DepthCache.resolutionX - .5f;
	float normalizedY = .5f - depthY / m_world_DepthCache.resolutionY;

	*pWorldX = normalizedX * depthZ * m_world_DepthCache.xzFactor;
	*pWorldY = normalizedY * depthZ * m_world_DepthCache.yzFactor;
	*pWorldZ = (float)depthZ;
}

void NIConverter::convertWorldToDepthCoordinates(float worldX, float worldY, float worldZ, float* pDepthX, float* pDepthY, float* pDepthZ)
{
	*pDepthX = m_world_DepthCache.coeffX * (worldX / worldZ) + m_world_DepthCache.halfResX;
	*pDepthY = m_world_DepthCache.halfResY - m_world_DepthCache.coeffY * (worldY / worldZ);
	*pDepthZ = worldZ;
}

void NIConverter::convertWorldToColorCoordinates(float worldX, float worldY, float worldZ, float* pColorX, float* pColorY)
{
	*pColorX = m_world_ColorCache.coeffX * (worldX / worldZ) + m_world_ColorCache.halfResX;
	*pColorY = m_world_ColorCache.halfResY - m_world_ColorCache.coeffY * (worldY / worldZ);
}

void NIConverter::convertDepthToColorCoordinates(float depthX, float depthY, float depthZ, int* pColorX, int* pColorY)
{
	float * pWorld = new float[3];
	float * pColor = new float[2];

	/*printf("depth %f, %f, %f \n",depthX,depthY,depthZ);*/
	convertDepthToWorldCoordinates(depthX,depthY,depthZ,pWorld,pWorld+1,pWorld+2);
	convertWorldToColorCoordinates(pWorld[0]+25,pWorld[1],pWorld[2],pColor,pColor+1);

	*pColorX= static_cast<int>(pColor[0]+0.5f); 
	*pColorY= static_cast<int>(pColor[1]+0.5f);
}